''*****************************************************
''*  Astronaut Glove Controller  v1.0                 *
''*  A space skydiving suit by Solar System Express   *
''*  Author: Blaze Sanders                            *
''*  Copyright (c) 2011 Solar System Express LLC      *
''*  See end of file for terms of use.                *
''************************************************************************************
''*  IMPORTANT NOTES:                                                                *
''*  > Longest valid continuous thurster firing is 4,294,967,295 system clock cycles * 
''*    caused by 32 bit counter overflow. Thus at standard 80 MHZ system clock       *
''*    longest possible firing is 53 seconds (long time). Thus run system at lowest  *       
''*    possible system, that fullfill reaction speed requirements.                   *                    
 
OBJ

DEBUG          : "SerialMirror"  
SOLENOIDVALVE  : "ZexProFuelSolenoidDriver"
GDB            : "GravityDevelopmentBoardDriver" 
CLK            : "Clock"
 
CON
'Declare finger enumerations. Correlates to hardware Parallax Pins used
#0, R_Palm, R_Pinky, R_RingF, R_MiddleF, R_IndexF, R_Thumb
#5, L_Palm, L_Pinky, L_RingF, L_MiddleF, L_IndexF, L_Thumb 
'Declare attitude enumerations
#20, PITCH, PitchUp, PitchDown, ROLL, RollClock, RollCClock, YAW, YawLeft, YawRight

'Declare retrothruster mode enumerations  
RetroGradeX2Mode = 2  'Slow rotation stop (Release thurster 66% of the way through rotation)
RetroGradeX4Mode = 4  'Medium rotation stop (Release thurster 80% of the way through rotation) 
RetroGradeX8Mode = 8  'Fast rotation stop (Release thurster 88% of the way through rotation) 


VAR
byte AttitudeFlags 'Five Least Significcant Bits (LSB) are not used 
byte DEBUG_MODE  
byte RetrogradeMultiplier
         
PUB Initialize(retrogradeMode)
DEBUG_MODE  := DEBUG#DEBUG_STATE
DEBUG.Str(STRING("Astronaut Glove Controller Initialization", DEBUG#CR))
SOLENOIDVALVE.Initialize  'Solenoid hardware and delta time control OBJECT
case retrogradeMode
  2: RetrogradeMultiplier := 2
  4: RetrogradeMultiplier := 4
  8: RetrogradeMultiplier := 8  


if(DEBUG_MODE)
  DEBUG.Str(STRING("AstronautGloveController Debug function is ON", DEBUG#CR)) 
  
return
 
PUB Main

''V1 = PitchDownValve
''V2 = PitchUpValve 


if(DEBUG_MODE)
  'DEBUG.Str(STRING("Astronaut Glove Main Function", DEBUG#CR)) 
  'CLK.PauseMSec(500)
  
if (SOLENOIDVALVE.CheckValveFlag(SOLENOIDVALVE#ValvePitchDown))  'if v1 == true
  if (INA[R_IndexF] & INA[R_Thumb])                                          'if f1 == true
    if(DEBUG_MODE)
      DEBUG.Str(STRING("DO NOTHING - PITCH DOWN VALVE ALREADY OPEN", DEBUG#CR))
  else'if(INA[R_Thumb] & (INA[R_MiddleF] | INA[R_RingF] | INA[R_Pinky] )) ???
    if(DEBUG_MODE)
      DEBUG.Str(STRING("start to close pitch down valve", DEBUG#CR))
      CLK.PauseMSec(3000) 
    SOLENOIDVALVE.ClosePitchDownValve(RetrogradeMultiplier)                                       'run method to close valve 1
else
  if ( (INA[R_IndexF] & INA[R_Thumb]) & Not(INA[R_MiddleF]) )                'if f1 == true && f2 == false
    DEBUG.Str(STRING("start to open pitch down valve", DEBUG#CR))
    SOLENOIDVALVE.OpenPitchDownValve(RetrogradeMultiplier)                                          'run method to open valve 1
  else
    if(DEBUG_MODE)
      DEBUG.Str(STRING("NO ACTIONS NEEDED #1", DEBUG#CR)) 


if (SOLENOIDVALVE.CheckValveFlag(SOLENOIDVALVE#ValvePitchUp))             'if v2 == true
  if (INA[R_MiddleF] & INA[R_Thumb])                                       'if f2 == true
    if(DEBUG_MODE)
      DEBUG.Str(STRING("DO NOTHING - PITCH UP VALVE ALREADY OPEN", DEBUG#CR))
  else'if(INA[R_Thumb] & (INA[R_IndexF] | INA[R_RingF] | INA[R_Pinky] )) ???
    if(DEBUG_MODE)
      DEBUG.Str(STRING("start to close pitch up valve", DEBUG#CR))
   SOLENOIDVALVE.ClosePitchUpValve(RetrogradeMultiplier)                                   'run method to close valve 2
else
  if ( (INA[R_MiddleF] & INA[R_Thumb]) & Not(INA[R_IndexF]) )             'if f2 == true && f1 == false
    SOLENOIDVALVE.OpenPitchUpValve(RetrogradeMultiplier)                                       'run method to open valve 2
  else
    if(DEBUG_MODE)
      DEBUG.Str(STRING("NO ACTIONS NEEDED #2", DEBUG#CR)) 

   
if (SOLENOIDVALVE.GetRG_PitchDownTime <> 0)                                               'if RG_Time_1 != 0
  DEBUG.Str(STRING("FIRE PitchDown RETRO ROCKETS", DEBUG#CR))
  SOLENOIDVALVE.OpenPitchDownValve(RetrogradeMultiplier)                                      'If there is any time left run RG_Thruster_1
if (SOLENOIDVALVE.GetRG_PitchUpTime  <> 0)                                                 'if RG_Time_2 != 0
  DEBUG.Str(STRING("FIRE PitchUp RETRO ROCKETS", DEBUG#CR))
  SOLENOIDVALVE.OpenPitchUpValve(RetrogradeMultiplier)                                      'If there is any time left run RG_Thruster_2            

PUB UnitTest
'Test each force sensor


PRI CheckAttitudeFlag(FlagType) | flag

case FlagType
  PITCH :
    flag := AttitudeFlags & %1000_0000
  YAW :
    flag := AttitudeFlags & %0100_0000  
  ROLL :
    flag := AttitudeFlags & %0010_0000 

if(DEBUG_MODE)
  DEBUG.Str(STRING("Flag ="))
  DEBUG.Str(flag)

return flag 'Non-zero flag equals TRUE, otherwise FALSE 


PRI RaiseAttitudeFlag(FlagType)

case FlagType
  PITCH :
   AttitudeFlags |= %1000_0000
  YAW :
    AttitudeFlags |= %0100_0000  
  ROLL :
    AttitudeFlags |= %0010_0000 

if(DEBUG_MODE)
  DEBUG.Str(STRING("Array ="))
  DEBUG.Str(AttitudeFlags)

PRI ClearValveFlag(FlagType)

case FlagType
  PITCH :
   AttitudeFlags &= %0111_1111
  YAW :
    AttitudeFlags &= %1011_1111   
  ROLL :
    AttitudeFlags &= %1101_1111 
 

if(DEBUG_MODE)
  DEBUG.Str(STRING("Flag Status ="))
  DEBUG.Str(AttitudeFlags)

  